#include "modules/CtrlModule/PassivePlanner/PostionStuckPlanner.h"
#include "lib/Time.h"

namespace  behavior_controller
{
    PostionStuckPlanner postion_stuck_planner;
    void PostionStuckPlanner::run(double &linear_vel, double &angular_vel)
    {
        if(time< 1e-4)
            time=Time::Now();
        if(Time::Now()-time< 0.5)
        {
            TurnLeft(linear_vel,angular_vel);
        }
        else if(Time::Now()-time< 1)
        {
            TurnRight(linear_vel,angular_vel);
        }
        else if(Time::Now()-time< 1.5)
        {
            Back(linear_vel,angular_vel);
        }
        else if(Time::Now()-time< 2)
        {
            TurnLeft(linear_vel,angular_vel);
        }
        else if(Time::Now()-time< 2.5)
        {
            TurnRight(linear_vel,angular_vel);
        }
        else if(Time::Now()-time< 3)
        {
            Back(linear_vel,angular_vel);
        }
        else if(Time::Now()-time< 3.5)
        {
            Foword(linear_vel,angular_vel);
        }
        else
        {
            Stop(linear_vel,angular_vel);
            finished=true;
            time=0;
        }
    }
    void PostionStuckPlanner:: Back(double &linear_vel, double &angular_vel)
    {
        linear_vel=-0.2;
        angular_vel=0;
    }
    void PostionStuckPlanner:: Foword(double &linear_vel, double &angular_vel)
    {
        linear_vel=0.2;
        angular_vel=0;
    }
    void PostionStuckPlanner:: TurnLeft(double &linear_vel, double &angular_vel)
    {
        linear_vel=0;
        angular_vel=0.4;
    }
    void PostionStuckPlanner:: TurnRight(double &linear_vel, double &angular_vel)
    {
        linear_vel=0;
        angular_vel=-0.4;
    }
    void PostionStuckPlanner:: Stop(double &linear_vel, double &angular_vel)
    {
        linear_vel=0;
        angular_vel=0;
    }
}